
#include "RWKinect.h"


using namespace std;
using namespace webots;


RWKinect::RWKinect()
{

}
RWKinect::~RWKinect()
{

}

void RWKinect::init(webots::Robot* R, ros::NodeHandle* n_in,string name)
{
  n = n_in;
  time_step = R->getBasicTimeStep();
  stringstream ss1, ss2;
  ss1 << name << "range";
  ss2 << name << "rgb";
  DEPCAM = R->getCamera(ss1.str());
  DEPCAM->enable(time_step);
  depcam_width = DEPCAM->getWidth();
  depcam_height = DEPCAM->getHeight();
  depcam_max_range = DEPCAM->getMaxRange();
  depcam_min_range = DEPCAM->getNear();
  depcam_hfoV = DEPCAM->getFov(); //horizontal field ov view in radians
  depcam_vfoV = depcam_hfoV * depcam_height/depcam_width;
  phi_min = -depcam_hfoV/2;
  theta_x_max = depcam_vfoV/2;
  //ACTIVE RGB CAMERA
  RGBCAM = R->getCamera(ss2.str());
  RGBCAM->enable(time_step);
  rgbcam_width = RGBCAM->getWidth();
  rgbcam_height = RGBCAM->getHeight();

  //ROS PUBLISHER CREATION
  std::stringstream DImage,DPoints,RCImage,RMImage,RPoints;
  //Publisher names
  string RobotName = R->getName();
  DImage << RobotName << "/kinect/depth/image";
  DPoints << RobotName << "/kinect/depth/points";
  RCImage << RobotName << "/kinect/rgb/image_color";
  RMImage << RobotName << "/kinect/rgb/image_mono";
  RPoints << RobotName << "/kinect/rgb/points";
  depcam_image_pub = n->advertise<sensor_msgs::Image>(DImage.str(), 1000);
  depcam_points_pub = n->advertise<sensor_msgs::PointCloud2>(DPoints.str(),1000);
  rgbcam_image_color_pub = n->advertise<sensor_msgs::Image>(RCImage.str(), 1000);
  rgbcam_image_mono_pub = n->advertise<sensor_msgs::Image>(RMImage.str(), 1000);
  rgbcam_points_pub = n->advertise<sensor_msgs::PointCloud2>(RPoints.str(),1000);

  //ROS MESSAGES
  DepcamImage.header.seq = 0;
  DepcamImage.header.frame_id = "kinect_range_lk";  //CHANGE!!!!!!
  DepcamImage.height = depcam_height;
  DepcamImage.width = depcam_width;
  DepcamImage.encoding = "32FC1";
  DepcamImage.is_bigendian = 0; //COMPROBAR CON DATOS REALES???
  DepcamImage.step = depcam_width * 4; //COMPROBAR QUE VA EN LAS FILAS
  DepcamImage.data.resize(depcam_height*depcam_width*4);

  RgbcamImageColor.header.seq = 0;
  RgbcamImageColor.header.frame_id = "kinect_rgb_lk";  //CHANGE!!!!!!
  RgbcamImageColor.height = rgbcam_height;
  RgbcamImageColor.width = rgbcam_width;
  RgbcamImageColor.encoding = "RGB8";
  RgbcamImageColor.is_bigendian = 0; //COMPROBAR CON DATOS REALES???
  RgbcamImageColor.step = rgbcam_width * 3; //COMPROBAR QUE VA EN LAS FILAS
  RgbcamImageColor.data.resize(rgbcam_height*rgbcam_width*3);


  RgbcamImageMono.header.seq = 0;
  RgbcamImageMono.header.frame_id = "kinect_rgb_lk";  //CHANGE!!!!!!
  RgbcamImageMono.height = rgbcam_height;
  RgbcamImageMono.width = rgbcam_width;
  RgbcamImageMono.encoding = "MONO8";  //Comprobar con kinect real
  RgbcamImageMono.is_bigendian = 0; //COMPROBAR CON DATOS REALES???
  RgbcamImageMono.step = rgbcam_width; //COMPROBAR QUE VA EN LAS FILAS
  RgbcamImageMono.data.resize(rgbcam_height*rgbcam_width);


  cloud_xyz.header.seq = 0;
  cloud_xyz.header.frame_id = "kinect_range_lk";
  cloud_xyzrgb.header = cloud_xyz.header;
  cloud_xyz.height = depcam_height;
  cloud_xyz.width = depcam_width;
  cloud_xyz.points.resize(depcam_height*depcam_width);
  cloud_xyzrgb.height = depcam_height;
  cloud_xyzrgb.width = depcam_width;
  cloud_xyzrgb.points.resize(depcam_height*depcam_width);



}



void RWKinect::iterate()
{
  ros::Time now = ros::Time::now();
  //DEPCAM IMAGE
  const float* depcam_image = DEPCAM->getRangeImage();
  memcpy(DepcamImage.data.data(),depcam_image,depcam_height*depcam_width*4);
  DepcamImage.header.seq++;
  DepcamImage.header.stamp = now;
  //COLOR IMAGE
  const unsigned char* rgbcam_image_color = RGBCAM->getImage();
  memcpy(RgbcamImageColor.data.data(),rgbcam_image_color,rgbcam_height*rgbcam_width*3);
  RgbcamImageColor.header.seq++;
  RgbcamImageColor.header.stamp = now;
  //MONO IMAGE
  unsigned char rgbcam_image_mono[(int)(rgbcam_height*rgbcam_width)];
  for (int k = 0;k< rgbcam_height*rgbcam_width;k++)
  {
    rgbcam_image_mono[k] = (unsigned char)((rgbcam_image_color[3*k]+rgbcam_image_color[3*k+1]+rgbcam_image_color[3*k+2]))/3;
  }
  memcpy(RgbcamImageMono.data.data(),rgbcam_image_mono,rgbcam_height*rgbcam_width);
  RgbcamImageMono.header.seq++;
  RgbcamImageMono.header.stamp = now;

  //PointClouds

  int i= 1;
  int j =1;
  int k2 = 0;
  int i2,j2;
  float r_mean,g_mean,b_mean;
  float r,phi,theta,px,py,pz;
  for(int k=0;k<depcam_height*depcam_width;k++)
  {
    r = depcam_image[k];
    phi = phi_min + (i-1)*(depcam_hfoV/depcam_width); //medido desde x en el plano xy
    theta = ((M_PI/2)-theta_x_max)+(j-1)*(depcam_vfoV/depcam_height);//medido desde z en plano xz

    px = r * sin(theta) * cos(phi);
    py = r * sin(theta) * sin(phi);
    pz = r * cos(theta);

    //As points defined from an optical point of view have z forward a change of axis is needed
    cloud_xyz.points[k].x = -py;
    cloud_xyz.points[k].y = -pz;
    cloud_xyz.points[k].z = px;
    cloud_xyzrgb.points[k].x = -py;
    cloud_xyzrgb.points[k].y = -pz;
    cloud_xyzrgb.points[k].z = px;

    if(depcam_width==rgbcam_width && depcam_height ==rgbcam_height)
    {
    	cloud_xyzrgb.points[k].r = (uint8_t)rgbcam_image_color[3*k];
    	cloud_xyzrgb.points[k].g = (uint8_t)rgbcam_image_color[3*k+1];
    	cloud_xyzrgb.points[k].b = (uint8_t)rgbcam_image_color[3*k+2];
    }
    else if((2*depcam_width)==rgbcam_width && (2*depcam_height) == rgbcam_height)
    {
      i2 = 2*i-1;
      j2 = 2*j-1;

      r_mean = g_mean = b_mean = 0;
      for (int k9 = 0;k9<4;k++)
      {
        k2 = (i2-1)*rgbcam_width+(j2-1);
        r_mean = r_mean + (float)rgbcam_image_color[3*k2];
        g_mean = g_mean + (float)rgbcam_image_color[3*k2+1];
        b_mean = b_mean + (float)rgbcam_image_color[3*k2+2];
        switch (k9)
        {
          case 0:
            i2++;
            break;
          case 1:
            j2++;
            break;
          case 2:
            i2--;
            break;
        }
      }
      cloud_xyzrgb.points[k].r = (uint8_t)r_mean;
      cloud_xyzrgb.points[k].g = (uint8_t)g_mean;
      cloud_xyzrgb.points[k].b = (uint8_t)b_mean;
    }
    j++;
    if (j>depcam_width)
    {j = 1;i++;}
  }
  cloud_xyz.header.seq++;
  cloud_xyz.header.stamp = now;
  cloud_xyzrgb.header.seq++;
  cloud_xyzrgb.header.stamp = now;
  pcl::toROSMsg(cloud_xyz,DepcamPoints);
  pcl::toROSMsg(cloud_xyzrgb,RgbcamPoints);


  //PUBLISH MESSAGES
  depcam_image_pub.publish(DepcamImage);
  rgbcam_image_color_pub.publish(RgbcamImageColor);
  rgbcam_image_mono_pub.publish(RgbcamImageMono);
  depcam_points_pub.publish(DepcamPoints);
  rgbcam_points_pub.publish(RgbcamPoints);

}



